#include <iostream>
#include <stdlib.h>
#include <SDL/SDL.h>
#include "Matrix4.h"

SDL_Surface *screen;
SDL_Surface *bitmap;

struct Point{
	int x;
	int y;
};

Point p1, p2, p3;
int tx=0, ty=0, tz=0;
float rx=0, ry=0, rz=0;
float scale = 1.;

void putPixel(SDL_Surface *surface, int x, int y, int pixel)
{
	if(x < 0 || y < 0) return;
	if(x >= surface->w || y >= surface->h) return;
    int bpp = surface->format->BytesPerPixel;
    /* Here p is the address to the pixel we want to set */
    Uint8 *p = (Uint8 *)surface->pixels + y * surface->pitch + x * bpp;

    switch(bpp) {
    case 1:
        *p = pixel;
        break;

    case 2:
        *(Uint16 *)p = pixel;
        break;

    case 3:
        if(SDL_BYTEORDER == SDL_BIG_ENDIAN) {
            p[0] = (pixel >> 16) & 0xff;
            p[1] = (pixel >> 8) & 0xff;
            p[2] = pixel & 0xff;
        } else {
            p[0] = pixel & 0xff;
            p[1] = (pixel >> 8) & 0xff;
            p[2] = (pixel >> 16) & 0xff;
        }
        break;

    case 4:
        *(Uint32 *)p = pixel;
        break;
    }
}

Uint32 getPixel(SDL_Surface *surface, int x, int y)
{
    int bpp = surface->format->BytesPerPixel;
    /* Here p is the address to the pixel we want to retrieve */
    Uint8 *p = (Uint8 *)surface->pixels + y * surface->pitch + x * bpp;

    switch(bpp) {
    case 1:
        return *p;

    case 2:
        return *(Uint16 *)p;

    case 3:
        if(SDL_BYTEORDER == SDL_BIG_ENDIAN)
            return p[0] << 16 | p[1] << 8 | p[2];
        else
            return p[0] | p[1] << 8 | p[2] << 16;

    case 4:
        return *(Uint32 *)p;

    default:
        return 0;       /* shouldn't happen, but avoids warnings */
    }
}

void line(SDL_Surface *surface, int x0, int y0, int x1, int y1, int value){
    float dx, dy, y, m;

    if(x0 > x1){
        int tmp; tmp = x0; x0 = x1; x1 = tmp;
        tmp = y0; y0 = y1; y1 = tmp;
    }
    dy = y1 - y0;
    dx = x1 - x0;
    if(dx == 0){
        for(int yy = y0 ; yy <= y1; yy++)
            putPixel(surface, x0, yy, value);
        return;
    }
    m = dy / dx;
    y = y0;
    for(int x = x0; x <= x1; x++){
        putPixel(surface, x, (int)(y + .5f), value);
        y += m;
    }
}

void line(SDL_Surface *surface, Point p0, Point p1, int value){
  line(surface, p0.x, p0.y, p1.x, p1.y, value);
}

void triangle(SDL_Surface *surface, Point p0, Point p1, Point p2, int value){
	line(surface, p0, p1, value);
	line(surface, p1, p2, value);
	line(surface, p0, p2, value);
}

void transform(SDL_Surface* surface, SDL_Surface *bitmap, float dx, float dy, float dz, float alphaX, float alphaY, float alphaZ, float scale){
	float out[3]= {0, 0, 0};

	Matrix4 matrix;
	matrix.translate(scale*bitmap->w/2,scale*bitmap->h/2,0.);
	matrix.translate(tx,ty,tz);
	matrix.rot(alphaZ, 'z');
	matrix.rot(alphaX, 'x');
	matrix.rot(alphaY, 'y');
	matrix.translate(-scale*bitmap->w/2,-scale*bitmap->h/2,0.);
	matrix.scale(scale,scale,1.);

	matrix.inverse();

	for(int i = 0; i < screen->w; i++)
        for(int j = 0; j < screen->h; j++){
            float in[] = {i, j, 0.};
            float out[3];
            matrix.multpoint3d(in, out);
			if(out[0] >= 0 && out[1] >=0 && out[0] < bitmap->w && out[1] < bitmap->h)
	            putPixel(screen, i, j, getPixel(bitmap, (int)out[0], (int)out[1]));
    }

}

void drawFrame(){
	transform(screen, bitmap, tx, ty, tz, rx, ry, rz, scale);
}

void keyDown(SDL_KeyboardEvent e){
	if(e.keysym.sym == SDLK_ESCAPE)	exit(0);
	if(e.keysym.sym == SDLK_LEFT)	tx-= 5;
	if(e.keysym.sym == SDLK_RIGHT)	tx+= 5;
	if(e.keysym.sym == SDLK_UP)		ty-=5;
	if(e.keysym.sym == SDLK_DOWN)	ty+=5;
	if(e.keysym.sym == SDLK_DELETE) tz-=5;
    if(e.keysym.sym == SDLK_PAGEUP) tz-=5;

    if(e.keysym.sym == SDLK_j) rx-= 5;
    if(e.keysym.sym == SDLK_l) rx+= 5;
    if(e.keysym.sym == SDLK_i) ry-=5;
    if(e.keysym.sym == SDLK_k) ry+=5;
    if(e.keysym.sym == SDLK_u) rz-=5;
    if(e.keysym.sym == SDLK_o) rz+=5;

	if(e.keysym.sym == SDLK_n) scale-=.05;
    if(e.keysym.sym == SDLK_m) scale+=.05;

}

int main(int argc, char* argv[]){ //Our main program
        SDL_Event event; //Events
        bool done = false; //Not done before we've started...
		int beginX, beginY, endX, endY;
		bitmap = SDL_LoadBMP("image.bmp");
		p1.x = 400; p1.y = 100;
		p2.x = 200; p2.y = 300;
		p3.x = 100; p3.y = 200;

		Matrix4 matrix4;
		matrix4.identity();

        if(SDL_Init(SDL_INIT_VIDEO) < 0){ //Could we start SDL_VIDEO?
                std::cerr << "Couldn't init SDL"; //Nope, output to stderr and quit
                exit(1);
        }

        atexit(SDL_Quit); //Now that we're enabled, make sure we cleanup
		SDL_EnableKeyRepeat(SDL_DEFAULT_REPEAT_DELAY, SDL_DEFAULT_REPEAT_INTERVAL);
        screen = SDL_SetVideoMode(800, 600, 32, SDL_HWSURFACE);

        while(!done){ //While program isn't done
                while(SDL_PollEvent(&event)){ //Poll events
                        switch(event.type){ //Check event type
                        case SDL_QUIT: //User hit the X (or equivelent)
                            done = true; //Make the loop end
                            break; //We handled the event
						case SDL_KEYDOWN:
							keyDown(event.key);
							break;
						case SDL_MOUSEBUTTONDOWN:
							break;
						case SDL_MOUSEBUTTONUP:
							break;
                        } //Finished with current event
                } //Done with all events for now
    SDL_LockSurface(screen);
	memset(screen->pixels, 0, 800*600*sizeof(uint));
    drawFrame();
    SDL_UnlockSurface(screen);
    SDL_UpdateRect(screen,0,0,0,0);

        } //Program done, exited
}
